#ifndef __SG_PCL_REGISTRATION_H__
#define __SG_PCL_REGISTRATION_H__

#include "SGPCLJsonConfig.h"

#include <pcl/common/transforms.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h> //使用随机样本一致性来识别inliers
#include <pcl/registration/icp.h> //ICP配准类相关头文件

namespace sgpcl
{

/// <summary>
/// 点云配准参数
/// </summary>
struct SGRegistrationConfig : public SGPCLJsonConfig
{
  size_t nMaximumIter;              // 最大迭代次数
  float fMaxCorrespondenceDistance; // 内点阈值

  SGPCL_API SGRegistrationConfig();
  SGPCL_API ~SGRegistrationConfig() = default;

  /// <summary>
  /// 从Json节点解析参数
  /// </summary>
  /// <param name="sJsonRoot"></param>
  /// <returns></returns>
  SGPCL_API void ParseJson(const rapidjson::Value& sJsonRoot) final override;
};

/// <summary>
/// 使用迭代最近点算法(ICP)
/// </summary>
/// <typeparam name="PointT"></typeparam>
/// <param name="spInputCloud"></param>
/// <param name="spModelCloud"></param>
/// <param name="sConfig"></param>
/// <returns></returns>
template <typename PointT>
Eigen::Affine3f ICP(typename pcl::PointCloud<PointT>::ConstPtr spInputCloud,
  typename pcl::PointCloud<PointT>::ConstPtr spModelCloud, const SGRegistrationConfig& sConfig)
{
  // 创建IterativeClosestPoint的实例
  // setInputSource将扫描断面作为输入点云
  // setInputTarget将设计断面作为目标点云
  pcl::IterativeClosestPoint<PointT, PointT> icp;
  icp.setInputSource(spInputCloud);
  icp.setInputTarget(spModelCloud);
  icp.setMaximumIterations(sConfig.nMaximumIter);                       //设置最大迭代次数
  icp.setMaxCorrespondenceDistance(sConfig.fMaxCorrespondenceDistance); //设置对应点之间的最大距离
  icp.setTransformationEpsilon(1e-8);
  icp.setEuclideanFitnessEpsilon(1);

  // 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象,存储配准变换后的源点云,
  // 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
  // （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp.
  // hasConverged()= 1 (true), 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
  pcl::PointCloud<PointT> Final;
  icp.align(Final);

  Eigen::Affine3f transformer(icp.getFinalTransformation());
  std::cout << "transformer:\n" << icp.getFinalTransformation() << '\n';

  // 可视化
  if (sConfig.IfVis())
  {
    pcl::visualization::PCLVisualizer::Ptr viewer(
      new pcl::visualization::PCLVisualizer("显示点云"));
    viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<PointT> target_color(spInputCloud, 255, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(spInputCloud, target_color, "target cloud");
    viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "target cloud");
    // 对源点云着色可视化 (green).
    pcl::visualization::PointCloudColorHandlerCustom<PointT> input_color(spModelCloud, 0, 255, 0);
    viewer->addPointCloud<PointT>(spModelCloud, input_color, "input cloud");
    viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "input cloud");
    // 对RANSAC后的点云着色可视化(blue)
    typename pcl::PointCloud<PointT>::Ptr spAfterRANSAC(new pcl::PointCloud<PointT>);
    pcl::transformPointCloud(*spInputCloud, *spAfterRANSAC, transformer);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> ransac_color(spAfterRANSAC, 0, 0, 255);
    viewer->addPointCloud<pcl::PointXYZ>(spAfterRANSAC, ransac_color, "ransac cloud");
    viewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "ransac cloud");
    viewer->initCameraParameters();
    while (!viewer->wasStopped())
    {
      viewer->spinOnce(100);
    }
  }
  return transformer;
}

/// <summary>
///
/// </summary>
/// <typeparam name="PointT"></typeparam>
/// <param name="spInputCloud"></param>
/// <param name="spModelCloud"></param>
/// <param name="sConfig"></param>
template <typename PointT>
int CorrespondenceEstimation(typename pcl::PointCloud<PointT>::ConstPtr spInitalCloud,
  typename pcl::PointCloud<PointT>::ConstPtr spTargetCloud, const SGRegistrationConfig& sConfig,
  Eigen::Affine3f& transformer, pcl::IndicesPtr& inliers)
{
  if (!inliers)
  {
    SG_PANIC("点云配准配对，内点指针为空！");
    return -1;
  }
  pcl::PointCloud<PointT>::Ptr spSourceCloud(new pcl::PointCloud<PointT>);
  pcl::copyPointCloud(*spInitalCloud, *spSourceCloud);
  transformer = Eigen::Affine3f::Identity();
  //---------初始化对象获取匹配点对----------------------

  float fMaxCorrDistance = sConfig.fMaxCorrespondenceDistance;
  float fOldMaxCorrDistance = fMaxCorrDistance;
  pcl::CorrespondencesPtr correspondence_inlier(new pcl::Correspondences);
  size_t nIter = 0;
  while (fMaxCorrDistance <= fOldMaxCorrDistance && fMaxCorrDistance > 1e-2 && nIter <= 10)
  {
    nIter++;
    pcl::registration::CorrespondenceEstimation<PointT, PointT> core;
    core.setInputSource(spSourceCloud);
    core.setInputTarget(spTargetCloud);

    pcl::CorrespondencesPtr correspondence_all(new pcl::Correspondences);
    core.determineCorrespondences(*correspondence_all); //确定输入点云与目标点云之间的对应关系

    //------------RANSAC----------------------------
    pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> ransac;
    ransac.setInputSource(spSourceCloud);
    ransac.setInputTarget(spTargetCloud);
    ransac.setMaximumIterations(sConfig.nMaximumIter); //设置最大迭代次数
    ransac.setInlierThreshold(fMaxCorrDistance);       //设置对应点之间的最大距离
    ransac.setRefineModel(true); //指定是否应该使用inliers的方差在内部细化模型
    ransac.getRemainingCorrespondences(*correspondence_all, *correspondence_inlier);
    Eigen::Affine3f step_transformer(ransac.getBestTransformation());
    Eigen::Matrix4f& mat = step_transformer.matrix();
    mat.block(0, 0, 3, 3) << Eigen::Matrix3f::
        Identity(); // 不让其旋转（检测车只会平移，认为扫描点云坐标系只有平移变换）
    transformer.matrix() = transformer.matrix() * step_transformer.matrix();

    // 计算配对点间的平均距离，作为下一次迭代的最大距离
    const size_t nPairNum = correspondence_inlier->size();
    float fAveDist = 0;
    for (const pcl::Correspondence& sPair : *correspondence_inlier)
    {
      fAveDist += sPair.distance / nPairNum;
    }
    fOldMaxCorrDistance = fMaxCorrDistance;
    fMaxCorrDistance = fAveDist;

    pcl::transformPointCloud(*spSourceCloud, *spSourceCloud, step_transformer); // 更新源点云
    // 可视化
    if (sConfig.IfVis())
    {
      //std::cout << "可视化！！\n";
      pcl::visualization::PCLVisualizer::Ptr viewer(
        new pcl::visualization::PCLVisualizer("foo!!!"));
      viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
      // 对目标点云着色可视化 (red).
      pcl::visualization::PointCloudColorHandlerCustom<PointT> target_color(
        spTargetCloud, 255, 0, 0);
      viewer->addPointCloud<PointT>(spTargetCloud, target_color, "target cloud");
      viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
      // 对源点云着色可视化 (green).
      pcl::visualization::PointCloudColorHandlerCustom<PointT> input_color(
        spInitalCloud, 0, 255, 0);
      viewer->addPointCloud<PointT>(spInitalCloud, input_color, "initial cloud");
      viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "initial cloud");
      // 对源点云着色可视化 (blue).
      pcl::visualization::PointCloudColorHandlerCustom<PointT> transformed_color(
        spSourceCloud, 0, 0, 255);
      viewer->addPointCloud<PointT>(spSourceCloud, transformed_color, "transformed cloud");
      viewer->setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "transformed cloud");
      //对应关系可视化
      viewer->addCorrespondences<PointT>(
        spInitalCloud, spTargetCloud, *correspondence_inlier, "correspondence");
      viewer->initCameraParameters();
      while (!viewer->wasStopped())
      {
        viewer->spinOnce(100);
      }
    }
  }

  // ----根据距离提取内点----
  pcl::registration::CorrespondenceEstimation<PointT, PointT> core;
  core.setInputSource(spSourceCloud);
  core.setInputTarget(spTargetCloud);

  pcl::CorrespondencesPtr correspondence_all(new pcl::Correspondences);
  core.determineCorrespondences(*correspondence_all, 0.3f); //确定输入点云与目标点云之间的对应关系
  inliers->clear();
  inliers->reserve(correspondence_all->size());
  for (const pcl::Correspondence&sCorr:*correspondence_all)
  {
    if (sCorr.index_match!=-1)
      inliers->push_back(sCorr.index_query);
  }

  // if (sConfig.IfVis())
  //{
  //   pcl::visualization::PCLVisualizer::Ptr viewer(
  //     new pcl::visualization::PCLVisualizer("bar!!!"));
  //   viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
  //   // 对目标点云着色可视化 (red).
  //   pcl::visualization::PointCloudColorHandlerCustom<PointT> target_color(spTargetCloud, 255, 0,
  //   0); viewer->addPointCloud<PointT>(spTargetCloud, target_color, "target cloud");
  //   viewer->setPointCloudRenderingProperties(
  //     pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target cloud");
  //   // 对源点云着色可视化 (blue).
  //   pcl::visualization::PointCloudColorHandlerCustom<PointT> transformed_color(
  //     spSourceCloud, 0, 0, 255);
  //   viewer->addPointCloud<PointT>(spSourceCloud, transformed_color, "transformed cloud");
  //   viewer->setPointCloudRenderingProperties(
  //     pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "transformed cloud");
  //   //对应关系可视化
  //   viewer->addCorrespondences<PointT>(
  //     spSourceCloud, spTargetCloud, *correspondence_all, "correspondence");
  //   viewer->initCameraParameters();
  //   while (!viewer->wasStopped())
  //   {
  //     viewer->spinOnce(100);
  //   }
  // }

  return 0;
}

} // namespace sgpcl
#endif // !__SG_PCL_REGISTRATION_H__
